// --------------------------------------------------------------------------------------------------------------------
// <copyright file="StartTrajectoryMessage.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Runtime.MotionControl
{
    using System;
    using System.Collections.Generic;
    using System.Collections.ObjectModel;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Message initiating a trajectory based motion
    /// </summary>
    [DataContract]
    public class StartTrajectoryMessage : AgentMessage
    {
        /// <summary>
        /// Sequence of waypoint times as a readonly collection.
        /// </summary>
        private ReadOnlyCollection<double> times;

        /// <summary>
        /// Initializes a new instance of the <see cref="StartTrajectoryMessage"/> class.
        /// </summary>
        /// <param name="originatingTime">Originating time.</param>
        /// <param name="numDOF">Number of degrees of freedom.</param>
        /// <param name="numPoints">Number of way points.</param>
        /// <param name="times">Waypoint times.  Must be monotonically increasing.</param>
        /// <param name="targetPositions">Waypoint positions indexed by [DOF, waypoint #].</param>
        /// <param name="targetVelocities">Waypoint velocities indexed by [DOF, waypoint #].</param>
        /// <param name="interpolationMethod">Interpolation method to use to determine positions between waypoints.</param>
        /// <param name="useCurrentPositionAsFirstPoint">
        /// Indicates whether or not to use the current 
        /// sensed velocity and position values for the first spline point.
        /// </param>
        /// <param name="adjustVelocitiesForContinuity">If supported by spline type, adjust target velocities to achieve continuity at higher derivatives </param>
        /// <param name="enforceZeroVelocityAtStartOfTrajectory">
        /// If true, do not adjust target velocity for first point with time > 0
        /// even if <c>AdjustVelocitiesForContinuity</c> is true.
        /// </param>
        public StartTrajectoryMessage(
            long originatingTime,
            int numDOF,
            int numPoints,
            double[] times,
            double[,] targetPositions,
            double[,] targetVelocities,
            TrajectoryInterpolationMethods interpolationMethod,
            bool useCurrentPositionAsFirstPoint,
            bool adjustVelocitiesForContinuity = false,
            bool enforceZeroVelocityAtStartOfTrajectory = true)
            : base(originatingTime)
        {
            if (numDOF <= 0)
            {
                throw new ArgumentException("numDOF must be greater than zero");
            }

            if (numPoints <= 1)
            {
                throw new ArgumentException("numPoints must be greater than one");
            }

            if (targetPositions.GetLength(0) != numDOF)
            {
                throw new ArgumentException(
                    string.Format(
                    "numDOF is {0}, but targetPositions contains {1} DOFs",
                        numDOF,
                        targetPositions.GetLength(0)));
            }

            if (targetVelocities.GetLength(0) != numDOF)
            {
                throw new ArgumentException(
                    string.Format(
                    "numDOF is {0}, but targetVelocities contains {1} DOFs",
                        numDOF,
                        targetVelocities.GetLength(0)));
            }

            if (targetPositions.GetLength(1) != numPoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "numPoints is {0}, but targetPositions contains {1} points",
                        numPoints,
                        targetPositions.GetLength(1)));
            }

            if (targetVelocities.GetLength(1) != numPoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "numPoints is {0}, but targetVelocities contains {1} points",
                        numPoints,
                        targetVelocities.GetLength(1)));
            }

            if (times.Length != numPoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "numPoints is {0}, but times contains {1} points",
                        numPoints,
                        times.Length));
            }

            this.NumDOF = numDOF;
            this.NumPoints = numPoints;
            this.InterpolationMethod = interpolationMethod;
            this.Times = Array.AsReadOnly<double>(times);
            this.TargetPositions = new TrajectoryWaypoints(targetPositions);
            this.TargetVelocities = new TrajectoryWaypoints(targetVelocities);
            this.UseCurrentPositionAsFirstPoint = useCurrentPositionAsFirstPoint;
            this.AdjustVelocitiesForContinuity = adjustVelocitiesForContinuity;
            this.EnforceZeroVelocityAtStartOfTrajectory = enforceZeroVelocityAtStartOfTrajectory;
        } 

        /// <summary>
        /// Gets number of degrees of freedom the trajectory controls
        /// </summary>
        [DataMember]
        public int NumDOF { get; private set; }

        /// <summary>
        /// Gets number of points in the trajectory.
        /// </summary>
        [DataMember]
        public int NumPoints { get; private set; }

        /// <summary>
        /// Gets a list of times in seconds indicating the target time of a trajectory point.
        /// For example, duration[n] is the amount of time in seconds from the start of the 
        /// trajectory to the point where it should reach target position n.
        /// </summary>
        [DataMember]
        public ReadOnlyCollection<double> Times 
        {
            get
            {
                return this.times;
            }

            private set
            {
                this.times = value;
            }
        }

        /// <summary>
        /// Gets target positions for each point in trajectory.  targetPositions[D, N] is the target position for degree of
        /// freedom D at point N.
        /// </summary>
        [DataMember]
        public TrajectoryWaypoints TargetPositions { get; private set; }

        /// <summary>
        /// Gets target velocities for each point in trajectory.  targetVelocity[D, N] is the target velocity for degree of freedom
        /// D at point N.  Must be zero for last point in trajectory.  Strongly recommended to be zero at any trajectory point where
        /// degree of freedom changes direction.  May not be used in all trajectory types.
        /// </summary>
        [DataMember]
        public TrajectoryWaypoints TargetVelocities { get; private set; }

        /// <summary>
        /// Gets method of interpolation to use to generate positions between waypoints.
        /// </summary>
        [DataMember]
        public TrajectoryInterpolationMethods InterpolationMethod { get; private set; }

        /// <summary>
        /// Gets a value indicating whether the position/velocity elements of the first waypoint should
        /// be overwritten with the current state before execution.
        /// </summary>
        [DataMember]
        public bool UseCurrentPositionAsFirstPoint { get; private set; }

        /// <summary>
        /// Gets a value indicating whether, if supported by spline type, adjust target velocities to achieve better continuity at higher derivatives
        /// </summary>
        [DataMember]
        public bool AdjustVelocitiesForContinuity { get; private set; }

        /// <summary>
        /// Gets a value indicating whether or not to adjust target velocity for first point with time > 0
        /// even if <c>AdjustVelocitiesForContinuity</c> is true.
        /// </summary>
        [DataMember]
        public bool EnforceZeroVelocityAtStartOfTrajectory { get; private set; }
    }
}
